#ifndef __MYMATH__
#define __MYMATH__

#include "main.h"
#define MAG_OFFSET_TIMES 5000
#define Gyro_R 0.0010653f // 数据乘以该参数
extern double BETA;
typedef enum
{
    IMU963RA,
    IMU660RA,
    ICM20948
} IMU_MODE_SELECT;

typedef struct
{
    float Input_Butter[3];
    float Output_Butter[3];
} Butter_BufferData;

typedef struct
{
    float a[3];
    float b[3];
} Butter_Parameter;

typedef struct
{
    float acce_x;
    float acce_y;
    float acce_z;
    float gyro_x;
    float gyro_y;
    float gyro_z;
} IMU_Offset_s;

typedef struct
{
    float gx_a, gy_a, gz_a; // 陀螺仪测量值（单位：angle/s）
    float gx_r, gy_r, gz_r; // 陀螺仪测量值（单位：rad/s）
    float gx_r_old, gy_r_old, gz_r_old;
    float ax, ay, az; // 加速度计测量值（单位：m/s^2）
    float ax_old, ay_old, az_old;
    float mx, my, mz;       // 地磁计测量值（单位：uT）
    float mx_f, my_f, mz_f; // 地磁计修正值
    IMU_Offset_s offset;    // 误差项
} imu_data_s;

//  roll          横滚角（围绕x轴转动），单位是°
//  pitch         俯仰角（围绕y轴转动），单位是°
//  yaw           偏航角（围绕z轴转动），单位是°
typedef struct
{
    float roll_a; // 角度制
    float roll_a_old;
    float pitch_a; // 角度制
    float pitch_a_old;
    float yaw_a; // 角度制
    float yaw_a_old;
    float yaw_s;   // 角度制
    float roll_r;  // 弧度制
    float pitch_r; // 弧度制
    float yaw_r;   // 弧度制

} EulerAngle_s;

typedef struct
{
    imu_data_s data;
    EulerAngle_s angle;
} imu_Data_All_s;

// 四元数结构体
typedef struct
{
    float q0;
    float q1;
    float q2;
    float q3;
    float q0_old;
    float q1_old;
    float q2_old;
    float q3_old;
} Quaternion;

// IMU相关数据总结构体
extern imu_Data_All_s IMU;
extern Quaternion q;

void IMU_Offset_Init(IMU_MODE_SELECT IMU_MODE);

void Madgwick_9_DOF_Get_Angle(IMU_MODE_SELECT IMU_MODE, uint8_t delta_t);

void Madgwick_AHRS_6_DOF_Get_Angle(double beta, uint8_t betaIn_is_true, IMU_MODE_SELECT IMU_MODE, uint8_t delta_t);
extern double BETA;
void imu963ra_get_value();
void AHRS_6_DOF_Get_Angle(IMU_MODE_SELECT IMU_MODE, float delta_T);

// 计算
#define PI 3.14159265358979f
#define RAD_TO_ANGLE(x) ((x) * 180.0 / PI) // 手动实现弧度转角度
double aabs(double value);
float invSqrt(float a);
void QuaternionNormalize(Quaternion *q);
void QuaternionToEuler(Quaternion *q, float *roll, float *pitch, float *yaw);
void Euler_ANGLE_To_REG(imu_Data_All_s *imu_in);
void Euler_REG_To_ANGLE(void);
double restrict_the_BETA(double b);

// 九轴
typedef struct
{
    int16_t i_gyro[3];
    int16_t i_acc[3];
    int16_t i_mag[3];
    float f_gyro[3];
    float f_acc[3];
    float f_mag[3];
    float mag_xsf;
    float mag_ysf;
    int16_t gyro_zero[3];
    int16_t acc_zero[3];
    int16_t mag_zero[3];

    float pitch;
    float roll;
    float yaw;
    float mag_yaw_test;
    float yaw_temp;
    float velocity[3];
    float displacement[3];
    uint8_t cali_flag;
    float accelerationMagnitude;
    uint8_t shell_cmd_ok;
    uint8_t output_mode;
    uint8_t output_flag;
    uint16_t output_freq;
    uint32_t time_tick;

} imu_t;

extern imu_t imu_9;

// 储存磁力计校准数据

typedef struct _MAGNETIC
{
    const int16_t xsf_buf_;
    const int16_t ysf_buf_;
    const int16_t mag_zerto_[3];
} _Magnetic;
extern _Magnetic mag_cal_;
float arctan(float x);
float arcsin(float x);
float myAtan2(float y, float x);
float myAsin(float x);
#endif
